#include "BridgeIOServer.hpp"
#include "EventHandlerUnknownConnection.hpp"
#include "cybertron/core/UtilTime.hpp"
#include "cybertron/core/UtilFile.hpp"
#include "cybertron/core/Thread.hpp"
#include "cybertron/core/Frame.hpp"
#include "cybertron/core/UtilString.hpp"
#include "Traffic/ScenarioEvent.pb.h"
#include "../Task/HotAreaTask/TaskHotAreaBase.hpp"
#include "../Util/BridgeIOConfig.hpp"
#include <thread>
#include <map>

BridgeIOServer::BridgeIOServer()
{
	UtilTime::millisecondsUpdate();
	zeroMembers();
}

BridgeIOServer::~BridgeIOServer()
{// do nothing.
	clearThis();
}

void BridgeIOServer::run()
{
	UtilTime::millisecondsUpdate();
	int t0 = UtilTime::millisecondsCurrent();

	for (;;)
	{
		UtilTime::millisecondsUpdate();
		int t1 = UtilTime::millisecondsCurrent();
		int ms = t1 - t0;
		t0 = t1;
		RunControlCommands();


		std::this_thread::sleep_for(std::chrono::microseconds(1));
	}
}

bool BridgeIOServer::initialize()
{
	clearThis();
	return true;
}

void BridgeIOServer::onAccept(SocketBasePtr pRemoteSocket)
{
	if (pRemoteSocket == nullptr)
	{
		return;
	}

	std::shared_ptr<EventHandlerUnknownConnection> pHandler =
		std::make_shared<EventHandlerUnknownConnection>(this);
	pRemoteSocket->addHandler(pHandler);
}



void BridgeIOServer::zeroMembers()
{
	mOutSystemClientNodesMap.clear();
}

void BridgeIOServer::clearThis()
{
	mControlCommandQueue.clear();
	zeroMembers();
}

void BridgeIOServer::queueControlCommand(
	SocketBasePtr pSender,
	const Message& msg)
{
	if (pSender == nullptr)
	{
		return;
	}

	Command cmd;
	cmd.pSender = pSender;
	cmd.msg = msg;

	mControlCommandQueue.push_back(cmd);
}

void BridgeIOServer::eraseDisconnected()
{

	std::set<SocketBasePtr> disconnected;
	{
		for (auto it = mUnKnowClientNodesMap.begin(); it != mUnKnowClientNodesMap.end();)
		{
			auto pSocket = it->second.mSocket;
			if (pSocket->getStatus() != ESocketStatus_Connected)
			{
				mUnKnowClientNodesMap.erase(it++);
			}
			else {
				++it;
			}
		}
		for (auto it = mMainVehicleClientNodesMap.begin(); it != mMainVehicleClientNodesMap.end(); )
		{
			auto pSocket = it->second.mSocket;
			if (pSocket->getStatus() != ESocketStatus_Connected)
			{
				mMainVehicleClientNodesMap.erase(it++);
			}
			else {
				++it;
			}
		}
		for (auto it = mOutSystemClientNodesMap.begin(); it != mOutSystemClientNodesMap.end();)
		{
			auto pSocket = it->second.mSocket;
			if (pSocket->getStatus() != ESocketStatus_Connected)
			{
				mOutSystemClientNodesMap.erase(it++);
			}
			else {
				++it;
			}
		}

	}

}

void BridgeIOServer::AddBridageClient(SocketBasePtr pSender, int role, int id, int sync, int type, std::string alias)
{
	std::string key = std::to_string(id).append("_").append(std::to_string(type));
	for (auto it = mUnKnowClientNodesMap.begin(); it != mUnKnowClientNodesMap.end(); it++)
	{
		if (it->first == key)
		{
			logWarning("A simulation(role:%d id:%d type:%d alias:%s) node has been registered %s:%d", role, id, type, alias.c_str(), pSender->getRemoteIp().c_str(), pSender->getRemotePort());
			while (1)
			{
				key = std::to_string(id).append("_").append(std::to_string(type));
				UnKnowClientNodesMap::iterator iter;
				iter = mUnKnowClientNodesMap.find(key);
				if (iter != mUnKnowClientNodesMap.end()) {
					id += 1;
				}
				else {
					break;
				}
			}
		}
	}

	for (auto it = mMainVehicleClientNodesMap.begin(); it != mMainVehicleClientNodesMap.end(); it++)
	{
		if (it->first == key)
		{
			logWarning("A simulation(role:%d id:%d type:%d alias:%s) node has been registered %s:%d", role, id, type, alias.c_str(), pSender->getRemoteIp().c_str(), pSender->getRemotePort());
			while (1)
			{
				key = std::to_string(id).append("_").append(std::to_string(type));
				UnKnowClientNodesMap::iterator iter;
				iter = mMainVehicleClientNodesMap.find(key);
				if (iter != mMainVehicleClientNodesMap.end()) {
					id += 1;
				}
				else {
					break;
				}
			}
		}
	}

	for (auto it = mOutSystemClientNodesMap.begin(); it != mOutSystemClientNodesMap.end(); it++)
	{
		if (it->first == key)
		{
			logWarning("A simulation(role:%d id:%d type:%d alias:%s) node has been registered %s:%d", role, id, type, alias.c_str(), pSender->getRemoteIp().c_str(), pSender->getRemotePort());
			while (true)
			{
				key = std::to_string(id).append("_").append(std::to_string(type));
				UnKnowClientNodesMap::iterator iter;
				iter = mOutSystemClientNodesMap.find(key);
				if (iter != mOutSystemClientNodesMap.end()) {
					id += 1;
				}
				else {
					break;
				}
			}
		}
	}

	{
		BridgeClientNode node;
		node.mSocket = pSender;
		node.role = role;
		node.id = id;
		node.type = type;
		node.isSync = sync;
		node.isConfirm = false;
		node.alias = alias;

		if (role == Bridge::EBridgeClientRole_SimOneOut) {
			mOutSystemClientNodesMap[key] = node;
			logInfo("REG===>Out(role:%d id:%d type:%d alias:%s) connect to bridgeIO %s:%d", node.role, node.id, node.type, node.alias.c_str(), pSender->getRemoteIp().c_str(), pSender->getRemotePort());
			return  ;
		}
		else if (role == Bridge::EBridgeClientRole_SimOneIn) {
			if (node.alias == "MainVehicle")
			{
				mMainVehicleClientNodesMap[key] = node;
				logInfo("REG===>IN(role:%d id:%d type:%d alias:%s)MainVehicle connect to bridgeIO %s:%d", node.role, node.id, node.type, node.alias.c_str(), pSender->getRemoteIp().c_str(), pSender->getRemotePort());
				return  ;
			}
			else
			{
				mUnKnowClientNodesMap[key] = node;
				logInfo("REG===>IN(role:%d id:%d type:%d alias:%s) ClientNodes connect to bridgeIO %s:%d", node.role, node.id, node.type, node.alias.c_str(), pSender->getRemoteIp().c_str(), pSender->getRemotePort());
				return  ;
			}
		}
		else {
			mUnKnowClientNodesMap[key] = node;
			logInfo("REG===>UnKnow Role(role:%d id:%d type:%d alias:%s) connect to bridgeIO %s:%d", node.role, node.id, node.type, node.alias.c_str(), pSender->getRemoteIp().c_str(), pSender->getRemotePort());
			return  ;
		}
		return  ;
	}

}

void BridgeIOServer::OnDisConnectClient(SocketBasePtr pSender) {
	if (pSender) {
		logInfo("OnDisConnectClient=======>%s:%d", pSender->getRemoteIp().c_str(), pSender->getRemotePort());
	}

	removeBridageClient(pSender);
	return;
}
void BridgeIOServer::FrameStep(int frame) {
	for (auto it = mOutSystemClientNodesMap.begin(); it != mOutSystemClientNodesMap.end(); it++)
	{
		auto pClientNode = &it->second;
		if (pClientNode->isSync) {
			if (pClientNode->mSocket->getStatus() == ESocketStatus_Connected) {
				pClientNode->isConfirm = false;
				Bridge::BridgeTimeStepForward msg;
				msg.set_framestamp(frame);
				pClientNode->mSocket->send(Bridge::EBridgeTimeStepForward, msg);
			}
		}
	}
	//
	int timeOutMilliseconds = -1;
	std::chrono::high_resolution_clock::time_point t1 = std::chrono::high_resolution_clock::now();
	while (1)
	{
		bool bFindDelayNode = false;
		int validCount = 0;
		for (auto it = mOutSystemClientNodesMap.begin(); it != mOutSystemClientNodesMap.end(); it++)
		{
			auto pClientNode = &it->second;
			if (pClientNode->isSync) {
				if (pClientNode->mSocket->getStatus() == ESocketStatus_Connected){
					validCount++;
					if (!pClientNode->isConfirm) {
						bFindDelayNode = true;
						//it->mSocket->getRemoteIp();
						//it->mSocket->getRemotePort();
						//it->alias;
					}
				}
			}
		}
		if (validCount == 0) {
			break;
		}
		if (!bFindDelayNode)
		{
			break;
		}
		else
		{
			//if (!mbAccurateNodeTimer)
			std::this_thread::sleep_for(std::chrono::milliseconds(1));
		}

		std::chrono::high_resolution_clock::time_point t2 = std::chrono::high_resolution_clock::now();
		std::chrono::nanoseconds span = (t2 - t1);
		if (timeOutMilliseconds > 0 && span.count() / 1000000 > timeOutMilliseconds)
		{
			// wait time out
			eraseDisconnected();
			return;
		}
	}
}
void BridgeIOServer::SendCaseStart() {
	Bridge::BridgeCaseStart start;
	Message msg;
	msg.fromProtobuf(Bridge::EBridgeCaseStart, start);
	BroadcastOutSystemNode(&msg);
	sendCaseStatusChange(ESimOne_Case_Status_Running);
}

void BridgeIOServer::SendCaseEnd() {
	Bridge::BridgeCaseEnd end;
	Message msg;
	msg.fromProtobuf(Bridge::EBridgeCaseEnd, end);
	BroadcastOutSystemNode(&msg);
	sendCaseStatusChange(ESimOne_Case_Status_Stop);

}

void BridgeIOServer::SendScenarioEvent(int mainVehicleId, const std::string * msgDataBody) {
	Bridge::BridgeScenarioEvent data;
	std::string* pData = data.mutable_buffer();
	pData->resize(msgDataBody->size());

	if (pData->size() > 0)
	{
		memcpy(&(pData->operator[](0)), (unsigned char*)msgDataBody->c_str(), msgDataBody->size());
	}
	Message msg;
	msg.fromProtobuf(Bridge::EBridgeScenarioEvent, data);
	SendHotareaDataSuberMainVehicle(mainVehicleId, &msg);
}
void BridgeIOServer::SendTrafficEvent(const std::string * msgDataBody) {
	Bridge::BridgeTrafficEvent data;
	std::string* pData = data.mutable_buffer();
	pData->resize(msgDataBody->size());

	if (pData->size() > 0)
	{
		memcpy(&(pData->operator[](0)), (unsigned char*)msgDataBody->c_str(), msgDataBody->size());
	}
	Message msg;
	msg.fromProtobuf(Bridge::EBridgeTrafficEvent, data);
	BroadcastOutSystemNode(&msg);
}

void BridgeIOServer::sendCaseStatusChange(int status)
{
	BridgeConfig::getInstance().SetCaseStatus(status);

	Bridge::BridgeCaseStatusChange data;
	data.set_status(status);
	Message msg;
	msg.fromProtobuf(Bridge::EBridgeCaseStatusChange, data);
	BroadcastOutSystemNode(&msg);
}

void BridgeIOServer::SendFrameStart(int frame) {
	Bridge::BridgeFrameStart start;
	start.set_framestamp(frame);
	Message msg;
	msg.fromProtobuf(Bridge::EBridgeFrameStart, start);
	BroadcastOutSystemNode(&msg);

}

void BridgeIOServer::SendEndTaskReason(int frame, const int endTaskReason) {
	Bridge::EndTaskReasonFromNodeTime data;
	data.set_endtaskreason(endTaskReason);
	data.set_hostvehicleid(BridgeConfig::getInstance().vehicleId());
	data.set_taskid(BridgeConfig::getInstance().getTaskId());
	float timeoneSec = 1.0 / BridgeConfig::getInstance().getVehicleHz();
	double time = frame * timeoneSec;

	data.set_time(time);
	Message msg;
	msg.fromProtobuf(Bridge::EEndTaskReasonFromNodeTime, data);
	BroadcastOutSystemNode(&msg);
}

void BridgeIOServer::SendMainVehicleStatus(int mainVehicleId, int ready) {
	Bridge::BridgeResultMainVehicleStatus result;
	result.set_mainvehicleid(mainVehicleId);
	result.set_issuccessful(1);
	result.set_mainvehiclestatus(ready);

	Message msg;
	msg.fromProtobuf(Bridge::EBridgeResultMainVehicleStatus, result);
	auto it = mSubMainVehicleMap.find(mainVehicleId);
	if (it != mSubMainVehicleMap.end())
	{
		for (auto i = 0; i < it->second.size(); i++)
		{
			it->second[i]->send(msg);
		}
	}
}
void BridgeIOServer::SendFrameEnd(int frame) {
	for (auto it = mOutSystemClientNodesMap.begin(); it != mOutSystemClientNodesMap.end(); it++)
	{
		auto pSocket = it->second;
		if (pSocket.mSocket->getStatus() == ESocketStatus_Connected) {

			Bridge::BridgeFrameEnd msg;
			msg.set_framestamp(frame);
			pSocket.mSocket->send(Bridge::EBridgeFrameEnd, msg);
		}
	}

	Bridge::BridgeFrameEnd end;
	end.set_framestamp(frame);
	Message msg;
	msg.fromProtobuf(Bridge::EBridgeFrameEnd, end);
	BroadcastOutSystemNode(&msg);
}

void BridgeIOServer::SendMessageEventWriteOut(SocketBasePtr pSender, void* pbuffer, int length, Bridge::EOutSimOneMessageType type) {
	Bridge::BridgeMessageFromSimOne2OutWriteReq writeReq;
	writeReq.mutable_header()->set_type(type);
	writeReq.set_buffer(pbuffer, length);
	Message msg;
	msg.fromProtobuf(Bridge::EBridgeMessageFromSimOne2OutWriteReq, writeReq);
	pSender->send(msg);
}
//
void BridgeIOServer::SendConfigReadResult(SocketBasePtr pSender, Message* pMessage, Bridge::EOutSimOneMessageType type){
	Bridge::BridgeMessageSimOneOutReadResult result;
	result.set_issuccessful(1);
	result.set_type(type);

	std::string* pData = result.mutable_buffer();

	pData->resize(pMessage->getDataBodyLength());

	if (pData->size() > 0)
	{
		memcpy(&(pData->operator[](0)), (unsigned char*)pMessage->getDataBody(), pMessage->getDataBodyLength());
	}

	Message msg;
	msg.fromProtobuf(Bridge::EBridgeMessageSimOneOutReadResult, result);
	pSender->send(msg);
}
void BridgeIOServer::SendConfigReadResult(SocketBasePtr pSender, void* pbuffer, int length, Bridge::EOutSimOneMessageType type) {
	Bridge::BridgeMessageSimOneOutReadResult result;
	result.set_issuccessful(1);
	result.set_type(type);

	std::string* pData = result.mutable_buffer();

	pData->resize(length);

	if (pData->size() > 0)
	{
		memcpy(&(pData->operator[](0)), pbuffer, length);
	}

	Message msg;
	msg.fromProtobuf(Bridge::EBridgeMessageSimOneOutReadResult, result);
	pSender->send(msg);
}
void BridgeIOServer::removeBridageClient(SocketBasePtr pSender) {
	for (auto item = mSubMainVehicleMap.begin(); item != mSubMainVehicleMap.end(); item++) {

		for (auto it = item->second.begin(); it < item->second.end(); it++) {
			if (*it == pSender) {
				item->second.erase(it);
				break;
			}
		}
		if (item->second.empty()) {
			mSubMainVehicleMap.erase(item);
			break;
		}
	}

	for (auto it = mMainVehicleClientNodesMap.begin(); it != mMainVehicleClientNodesMap.end(); it++)
	{
		if (it->second.mSocket == pSender)
		{
			mMainVehicleClientNodesMap.erase(it);
			return;
		}
	}
	for (auto it = mOutSystemClientNodesMap.begin(); it != mOutSystemClientNodesMap.end(); it++)
	{
		if (it->second.mSocket == pSender)
		{
			mOutSystemClientNodesMap.erase(it);
			return;
		}
	}
	for (auto it = mUnKnowClientNodesMap.begin(); it != mUnKnowClientNodesMap.end(); it++)
	{
		if (it->second.mSocket == pSender)
		{
			mUnKnowClientNodesMap.erase(it);
			return;
		}
	}

}

std::uint32_t  BridgeIOServer::GetSensorType(std::uint32_t sensorType) {
	std::uint32_t type = 0;
	switch (sensorType)
	{
	case Bridge::ESensorType_Vehicle:
		type = ESimOne_Node_Type_Vehicle;
		break;
	case Bridge::ESensorType_Camera:
		type = ESimOne_Node_Type_Camera;
		break;
	case Bridge::ESensorType_LiDAR:
		type = ESimOne_Node_Type_LiDAR;
		break;
	case Bridge::ESensorType_MMWRadar:
		type = ESimOne_Node_Type_MMWRadar;
		break;
	//case Bridge::ESensorType_UltrasonicRadar:
	//	type = ESimOneNode_UltrasonicRadar;
	//	break;
	case Bridge::ESensorType_AllUltrasonicRadar:
		type = ESimOne_Node_Type_AllUltrasonicRadar;
		break;
	case Bridge::ESensorType_GNSSINS:
		type = ESimOne_Node_Type_GNSSINS;
		break;
	case Bridge::ESensorType_PerfectPerception:
		type = ESimOne_Node_Type_PerfectPerception;
		break;
	case Bridge::ESensorType_V2X:
		type = ESimOne_Node_Type_V2X;
		break;

	default:
		break;
	}
	return type;
}
void BridgeIOServer::RunControlCommands()
{
	Command cmd;
	while (mControlCommandQueue.pop_swap_front(cmd))
	{
		executeControlCommand(cmd);
	}
}

//
void BridgeIOServer::executeControlCommand(
	Command& cmd)
{
	std::uint16_t msgId = cmd.msg.parseMsgId();
	switch (msgId)
	{
	case Bridge::EBridgeNodeRegisterReq:
		onFromBridgeNodeRegisterReq(cmd);
		break;

	case Bridge::EBridgeSimOneAPIReady:
		onFromBridgeSimOneAPIReady(cmd);
		break;
	case Bridge::EBridgeReqSubMainVehicle:
		onFromBridgeReqSubMainVehicle(cmd);
		break;
	case Bridge::EBridgeReqMainVehicleList:
		//onFromBridgeMainVehicleListMessage(cmd);
		break;
	case Bridge::EBridgeReqMainVehicleStatus:
		onFromBridgeMainVehicleStatus(cmd);
		break;
	case Bridge::ESensorAnyDataMessage:
		onFromSensorAnyDataMessage(cmd);
		break;
	case Bridge::EMainVehicleAnyDataMessage:
		onFromMainVehicleDataMessage(cmd);
		break;
	case Bridge::EBridgeTimeStepForwardConfirm:
		onFromBridgeTimeStepForwardConfirmDataMessage(cmd);
		break;
	case Bridge::EBridgeMessageFromOut2SimOneWriteReq:
		onFromBridgeMessageFromOut2SimOneWriteReq(cmd);
		break;
	case Bridge::EBridgeMessageSimOneOutReadReq:
		onFromBridgeMessageSimOneOutReadReq(cmd);
		break;
	case Bridge::EBridgeMessageSimOneOutReadResult:
		onFromBridgeMessageSimOneOutReadResult(cmd);
		break;
	case Bridge::EBridgeDataRoute:
		onFromBridgeDataRouteMessage(cmd);
		break;
	default:
		break;
	}
}
void BridgeIOServer::onFromBridgeNodeRegisterReq(Command& cmd) {
    Bridge::BridgeNodeRegisterResult declareResult;

	Bridge::BridgeNodeRegister role;
	if (!cmd.msg.toProtobuf(role))
	{
        logInfo("===failed BridgeIOServer::onFromBridgeNodeRegisterReq() 1");
        declareResult.set_succeeded(false);
        cmd.pSender->send(Bridge::EBridgeNodeRegisterResult, declareResult);
		return;
	}

    while (true)
    {
        bool is_get_task = BridgeConfig::getInstance().IsGetTask();
        if (is_get_task) {
            logInfo("============>task is ok========>");
            break;
        }
        std::this_thread::sleep_for(std::chrono::microseconds(1000));
        logInfo("============>wait task ready========>");
    }

	declareResult.set_succeeded(true);
	if (role.role() == Bridge::EBridgeClientRole_SimOneOut) {
		SendEnvironmentInfo(cmd.pSender, Bridge::EOutSimOneMessageType_Environment);
		SendSensorConfigurations(cmd.pSender, Bridge::EOutSimOneMessageType_SensorConfigurations);
		SendWayPoints(cmd.pSender, Bridge::EOutSimOneMessageType_WayPoints);
		SendHDMapInfo(cmd.pSender, Bridge::EOutSimOneMessageType_MapInfo);
		SendCaseInfo(cmd.pSender, Bridge::EOutSimOneMessageType_CaseInfo);
		SendCaseStatus(cmd.pSender, Bridge::EOutSimOneMessageType_CaseStatus);
		SendMainVehicleList(cmd.pSender);
	}

	cmd.pSender->send(Bridge::EBridgeNodeRegisterResult, declareResult);
	//AddBridageClient(cmd.pSender, role.role(), role.id(), false, role.alias());
	AddBridageClient(cmd.pSender, role.role(), role.id(), false, role.type(), role.alias());
	//SendEnvironmentInfo(cmd.pSender);
}
void BridgeIOServer::onFromBridgeReqSubMainVehicle(Command& cmd) {
	Bridge::BridgeReqSubMainVehicle info;
	if (!cmd.msg.toProtobuf(info))
	{
		return;
	}
	int mainvehicleid = info.mainvehicleid();
	TaskHotAreaManager::MainVehicleStatus* pMainVehicle =  TaskHotAreaManager::getInstance().GetMainVehicleStatus(mainvehicleid);
	Bridge::BridgeResultSubMainVehicle result;
	result.set_mainvehicleid(mainvehicleid);
	if (pMainVehicle) {
		result.set_issuccessful(1);
	}
	else {
		result.set_issuccessful(0);
		cmd.pSender->send(Bridge::EBridgeResultSubMainVehicle, result);
		return;
	}
	cmd.pSender->send(Bridge::EBridgeResultSubMainVehicle, result);
	auto it = mSubMainVehicleMap.find(mainvehicleid);
	if (it == mSubMainVehicleMap.end())
	{
		std::vector<SocketBasePtr> SocketV;
		SocketV.push_back(cmd.pSender);
		mSubMainVehicleMap[mainvehicleid] = SocketV;
	}
	else
	{
		if (it->first == mainvehicleid)
		{
			it->second.push_back(cmd.pSender);
		}
	}

	for (auto it = mOutSystemClientNodesMap.begin(); it != mOutSystemClientNodesMap.end(); it++)
	{
		if (it->second.mSocket == cmd.pSender)
		{
			it->second.isSync = info.isjointimeloop();
			break;
		}
	}
}
void BridgeIOServer::onFromBridgeSimOneAPIReady(Command& cmd) {
	logInfo("====>onFromBridgeSimOneAPIReady");
	Bridge::BridgeSimOneAPIReady msg;
    if (!cmd.msg.toProtobuf(msg)){
       return;
    }
    int hostVehicle = msg.hostvehicleid();
    BridgeConfig::getInstance().AddSimOneAPICount(std::to_string(hostVehicle));
}
void BridgeIOServer::onFromBridgeTimeStepForwardConfirmDataMessage(Command& cmd) {
	Bridge::BridgeTimeStepForwardConfirm msg;
	if (!cmd.msg.toProtobuf(msg))
	{
		return;
	}

	for (auto it = mOutSystemClientNodesMap.begin(); it != mOutSystemClientNodesMap.end(); it++)
	{
		auto pClient = &it->second;
		if (pClient->mSocket == cmd.pSender)
		{

			pClient->isConfirm = true;
			break;
		}
	}
	return;
}
void BridgeIOServer::onFromSensorAnyDataMessage(Command& cmd) {
	Bridge::SensorAnyDataMessage sensorMessage;
	if (!cmd.msg.toProtobuf(sensorMessage)){
		return;
	}
	Bridge::BridgeSensorHeader *header = sensorMessage.mutable_header();
	auto hostVehicleId = header->mainvehicleid();
	SendHotareaDataSuberMainVehicle(hostVehicleId, &cmd.msg);
}

void BridgeIOServer::onFromMainVehicleDataMessage(Command& cmd) {
	Bridge::MainVehicleAnyDataMessage mainVehicleMessage;
	if (!cmd.msg.toProtobuf(mainVehicleMessage))
	{
		return;
	}
	Bridge::BridgeMainVehicleHeader header = mainVehicleMessage.header();
	int mainvehicleid = header.mainvehicleid();
	switch (header.commandid())
	{
	case proto::sensor::EDataType_VehicleBodyState:
		onFromBridgeVehicleBodyState(mainvehicleid,cmd);
		break;
	case proto::sensor::EDataType_VehicleControlState:
		onFromBridgeVehicleControlState(mainvehicleid,cmd);
		break;
	case proto::sensor::EDataType_VehicleExtraStateIndics:
		onFromBridgeVehicleExtraState(mainvehicleid, cmd);
		break;
	case proto::sensor::EDataType_VehicleEventInfo:
		onFromBridgeVehicleEventInfo(mainvehicleid,cmd);
		break;
	case proto::sensor::EDataType_VehicleTrajectoryState:
		onFromBridgeVehicleTrajectoryState(mainvehicleid,cmd);
		break;
	case proto::sensor::EDataType_SignalLights:
		onFromSignalLights(cmd);
		break;
		//
	case proto::sensor::EDataType_DriverStatus:
		onFromDriverStatus(cmd);
		break;
	case proto::sensor::EDataType_DriverControl:
		onFromDriverControl(cmd);
		break;
	case proto::sensor::EDataType_VehicleControlMode:
		onFromControlMode(cmd);
		break;
	default:
		break;
	}
}

void BridgeIOServer::onFromBridgeVehicleBodyState(int hostVehicleId,Command& cmd) {
	//BroadcastMainVehicleNode(&cmd.msg);
	SendToSimOneInMainVehicleNode(&cmd.msg, hostVehicleId, Bridge::EBridgeClientType_VehicleDynamic);
}
void BridgeIOServer::onFromBridgeVehicleExtraState(int hostVehicleId, Command& cmd) {
	//BroadcastMainVehicleNode(&cmd.msg);
	SendToSimOneInMainVehicleNode(&cmd.msg, hostVehicleId, Bridge::EBridgeClientType_VehicleDynamic);
}
void BridgeIOServer::onFromBridgeVehicleControlState(int hostVehicleId, Command& cmd) {
	//BroadcastMainVehicleNode(&cmd.msg);
	SendToSimOneInMainVehicleNode(&cmd.msg, hostVehicleId, Bridge::EBridgeClientType_VehicleDynamic);
}
void BridgeIOServer::onFromBridgeVehicleEventInfo(int hostVehicleId, Command& cmd) {
	//BroadcastMainVehicleNode(&cmd.msg);
	SendToSimOneInMainVehicleNode(&cmd.msg, hostVehicleId, Bridge::EBridgeClientType_VehicleDynamic);
}
void BridgeIOServer::onFromBridgeVehicleTrajectoryState(int hostVehicleId, Command& cmd) {
	//BroadcastMainVehicleNode(&cmd.msg);
	SendToSimOneInMainVehicleNode(&cmd.msg, hostVehicleId, Bridge::EBridgeClientType_VehicleDynamic);
}
void BridgeIOServer::onFromDriverStatus(Command& cmd) {
	BroadcastOutSystemNode(&cmd.msg);
}
void BridgeIOServer::onFromDriverControl(Command& cmd) {
	BroadcastOutSystemNode(&cmd.msg);
}
void BridgeIOServer::onFromControlMode(Command& cmd) {
	BroadcastOutSystemNode(&cmd.msg);
}
void BridgeIOServer::onFromBridgeMessageFromOut2SimOneWriteReq(Command& cmd) {

	Bridge::BridgeMessageFromSimOne2OutWriteReq state;
	if (!cmd.msg.toProtobuf(state))
	{
		return;
	}
	switch (state.header().type()) {
	case 0:
		onFromEnvironmentInfo(cmd, &(state.buffer()));
		break;
	default:
		break;
	}
}

void BridgeIOServer::onFromBridgeMessageSimOneOutReadReq(Command& cmd) {
	Bridge::BridgeMessageSimOneOutReadReq state;
	if (!cmd.msg.toProtobuf(state))
	{
		return;
	}
	int type = state.type();
	switch (type)
	{
	case Bridge::EOutSimOneMessageType_Environment:
		ReadEnvironmentInfo(cmd.pSender, state.type());
		break;
	case Bridge::EOutSimOneMessageType_SensorConfigurations:
		ReadSensorConfigurations(cmd.pSender, state.type());
		break;
	case Bridge::EOutSimOneMessageType_WayPoints:
		ReadWayPoints(cmd.pSender, state.type());
		break;
	case Bridge::EOutSimOneMessageType_MapInfo:
		//ReadMapInfo(cmd.pSender,cmd);
		break;
	case Bridge::EOutSimOneMessageType_CaseInfo:
		//ReadCaseInfo(cmd.pSender,cmd);
		break;
	default:
		break;
	}
}

void BridgeIOServer::onFromBridgeMessageSimOneOutReadResult(Command& cmd) {
	//SendSensorConfigurations(cmd.pSender);
}

void BridgeIOServer::onFromBridgeDataRouteMessage(Command& cmd) {
	Bridge::BridgeDataRoute data;
	if (!cmd.msg.toProtobuf(data))
	{
		return;
	}
	int toId = data.header().toid();
	int toType = data.header().totype();
	int commandId = data.header().commandid();
	std::string key = std::to_string(toId).append("_").append(std::to_string(toType));

	UnKnowClientNodesMap::iterator itemUnKnowClient = mUnKnowClientNodesMap.find(key);
	if (itemUnKnowClient != mUnKnowClientNodesMap.end())
	{
		itemUnKnowClient->second.send(cmd.msg);
	}
	MainVehicleClientNodesMap::iterator itemMainVehicleClient = mMainVehicleClientNodesMap.find(key);
	if (itemMainVehicleClient != mMainVehicleClientNodesMap.end())
	{
		itemMainVehicleClient->second.send(cmd.msg);
	}
	OutSystemClientNodesMap::iterator OutSystemClient = mOutSystemClientNodesMap.find(key);
	if (OutSystemClient != mOutSystemClientNodesMap.end())
	{
		OutSystemClient->second.send(cmd.msg);
	}
}
void BridgeIOServer::onFromSignalLights(Command& cmd) {
	BroadcastAllNode(&cmd.msg);
}

void BridgeIOServer::onFromBridgeMainVehicleListMessage(Command& cmd) {
	Bridge::BridgeResultMainVehicleList MainVehicleInfo;

    std::map<int, std::string>::iterator  it;
	for (it = mMainVehicleInfo.begin();  it != mMainVehicleInfo.end(); it++)
	{
		auto item = MainVehicleInfo.add_mainvehicle_info();
		item->set_id(it->first);
		item->set_type(it->second);
	}

	Message msg;
	msg.fromProtobuf(Bridge::EBridgeResultMainVehicleList, MainVehicleInfo);
	cmd.pSender->send(msg);
}
void BridgeIOServer::onFromBridgeMainVehicleStatus(Command& cmd){
	Bridge::BridgeReqMainVehicleStatus req;
	if (!cmd.msg.toProtobuf(req))
	{
		return;
	}
	int mainvehicleid = req.mainvehicleid();

	TaskHotAreaManager::MainVehicleStatus* pMainVehicleStatus= TaskHotAreaManager::getInstance().GetMainVehicleStatus(mainvehicleid);

	Bridge::BridgeResultMainVehicleStatus result;
	result.set_mainvehicleid(mainvehicleid);
	if (!pMainVehicleStatus) {
		result.set_issuccessful(0);
		result.set_mainvehiclestatus(0);
	}
	else {
		result.set_issuccessful(1);
		result.set_mainvehiclestatus(pMainVehicleStatus->IsReady);
	}

	Message msg;
	msg.fromProtobuf(Bridge::EBridgeResultMainVehicleStatus, result);
	cmd.pSender->send(msg);
}

void BridgeIOServer::BroadcastMainVehicleNode(Message* pMessage) {
	for (auto it = mMainVehicleClientNodesMap.begin(); it != mMainVehicleClientNodesMap.end(); it++)
	{
		it->second.send(*pMessage);
	}
}

void BridgeIOServer::BroadcastAllNode(Message* pMessage) {
	for (auto it = mMainVehicleClientNodesMap.begin(); it != mMainVehicleClientNodesMap.end(); it++)
	{
		it->second.send(*pMessage);
	}

	for (auto it = mUnKnowClientNodesMap.begin(); it != mUnKnowClientNodesMap.end(); it++)
	{
		it->second.send(*pMessage);
	}

	for (auto it = mOutSystemClientNodesMap.begin(); it != mOutSystemClientNodesMap.end(); it++)
	{
		it->second.send(*pMessage);
	}
}
void BridgeIOServer::SendToSimOneInMainVehicleNode(Message* pMessage, int id, Bridge::EBridgeClientType type) {
	std::string key = std::to_string(id).append("_").append(std::to_string(type));
	MainVehicleClientNodesMap::iterator it = mMainVehicleClientNodesMap.find(key);
	if (it != mMainVehicleClientNodesMap.end())
	{
		it->second.send((*pMessage));
	}
}

//
void BridgeIOServer::BroadcastOutSystemNode(Message* pMessage) {
	for (auto it = mOutSystemClientNodesMap.begin(); it != mOutSystemClientNodesMap.end(); it++)
	{
		BridgeClientNode* pClient = &it->second;
		if (pClient == nullptr || pClient->mSocket == nullptr) {
			continue;
		}
		if (pClient->mSocket->getStatus() == ESocketStatus_Connected) {
			pClient->send((*pMessage));
		}
	}
}

void BridgeIOServer::SendHotareaDataSuberMainVehicle(int mainVehicleId, Message* pMessage) {
	auto it = mSubMainVehicleMap.find(mainVehicleId);
	if (it != mSubMainVehicleMap.end()){
		for (auto i = 0; i < it->second.size(); i++){
			it->second[i]->send((*pMessage));
		}
	}
}
//
void BridgeIOServer::SendToOutSystemNode(Message* pMessage, int id, Bridge::EBridgeClientType type) {

	std::string key = std::to_string(id).append("_").append(std::to_string(type));
	OutSystemClientNodesMap::iterator it = mOutSystemClientNodesMap.find(key);
	if (it != mOutSystemClientNodesMap.end())
	{
		BridgeClientNode* pClient = &it->second;
		if (pClient->mSocket->getStatus() == ESocketStatus_Connected) {
			pClient->send((*pMessage));
		}
		it->second.send((*pMessage));
	}
}

void BridgeIOServer::ReadEnvironmentInfo(SocketBasePtr pSender, Bridge::EOutSimOneMessageType type) {
	const SimOne_Data_Environment pRet = BridgeConfig::getInstance().sensorWeatherConfig();
	SendConfigReadResult(pSender, (void*)&pRet, sizeof(SimOne_Data_Environment), type);
}

void BridgeIOServer::ReadSensorConfigurations(SocketBasePtr pSender, Bridge::EOutSimOneMessageType type) {

	auto hostVehicleConfig = BridgeConfig::BridgeConfig::getInstance().getHostVehicleConfig();
	for (auto item = hostVehicleConfig.begin(); item != hostVehicleConfig.end(); item++) {
		const SimOne_Data_SensorConfigurations *pRet = BridgeConfig::getInstance().SensorConfigurations(item->first);
		SendConfigReadResult(pSender, (void*)pRet, sizeof(SimOne_Data_SensorConfigurations), type);
	}
}

void BridgeIOServer::ReadWayPoints(SocketBasePtr pSender, Bridge::EOutSimOneMessageType type) {
	auto hostVehicleConfig = BridgeConfig::BridgeConfig::getInstance().getHostVehicleConfig();
	for (auto item = hostVehicleConfig.begin(); item != hostVehicleConfig.end(); item++) {
		SimOne_Data_WayPoints *pRet = BridgeConfig::getInstance().wayPointsConfig(item->first);
		SendConfigReadResult(pSender, (void*)pRet, sizeof(SimOne_Data_WayPoints), type);
	}
}
void BridgeIOServer::onFromEnvironmentInfo(Command& cmd, const std::string* msgDataBody)
{
	SimOne_Data_Environment* pWeatherConfig = (SimOne_Data_Environment*)msgDataBody->c_str();
	SimOne_Data_Environment newWeatherConfig;
	newWeatherConfig.timeOfDay =  pWeatherConfig->timeOfDay;
	newWeatherConfig.directionalLight = pWeatherConfig->directionalLight;
	newWeatherConfig.ambientLight = pWeatherConfig->ambientLight;
	newWeatherConfig.artificialLight = pWeatherConfig->artificialLight;
	newWeatherConfig.heightAngle = pWeatherConfig->heightAngle;
	newWeatherConfig.cloudDensity = pWeatherConfig->cloudDensity;
	newWeatherConfig.fogDensity = pWeatherConfig->fogDensity;
	newWeatherConfig.rainDensity = pWeatherConfig->rainDensity;
	newWeatherConfig.snowDensity = pWeatherConfig->snowDensity;
	newWeatherConfig.groundHumidityLevel = pWeatherConfig->groundHumidityLevel;
	newWeatherConfig.groundDirtyLevel = pWeatherConfig->groundDirtyLevel;
	BridgeConfig::getInstance().SetEnvironment(pWeatherConfig);


	Bridge::BridgeMessageFromOut2SimOneWriteReq req;

	Bridge::WriteOutSimOneMessageTypeHeader *header = req.mutable_header();
	header->set_type(Bridge::EOutSimOneMessageType_Environment);

	std::string* pData = req.mutable_buffer();
	pData->resize(msgDataBody->size());

	if (pData->size() > 0)
	{
		memcpy(&(pData->operator[](0)), (unsigned char*)msgDataBody->c_str(), msgDataBody->size());
	}

	Message msg;
	msg.fromProtobuf(Bridge::EBridgeMessageFromOut2SimOneWriteReq, req);

	BroadcastAllNode(&msg);

	Bridge::BridgeMessageFromOut2SimOneWriteResult result;
	result.set_issuccessful(1);

	Message resultMessage;
	resultMessage.fromProtobuf(Bridge::EBridgeMessageFromOut2SimOneWriteResult, result);
	cmd.pSender->send(resultMessage);
}
void BridgeIOServer::SendEnvironmentInfo(SocketBasePtr pSender, Bridge::EOutSimOneMessageType type)
{
	const SimOne_Data_Environment pRet = BridgeConfig::getInstance().sensorWeatherConfig();
	SendMessageEventWriteOut(pSender, (void*)&pRet, sizeof(SimOne_Data_Environment), type);
}
void BridgeIOServer::SendSensorConfigurations(SocketBasePtr pSender, Bridge::EOutSimOneMessageType type)
{
	auto hostVehicleConfig = BridgeConfig::BridgeConfig::getInstance().getHostVehicleConfig();
	for (auto item = hostVehicleConfig.begin(); item != hostVehicleConfig.end(); item++) {
		SimOne_Data_SensorConfigurations *pRet = BridgeConfig::getInstance().SensorConfigurations(item->first);
		for (int i = 0; i < pRet->dataSize; i++)
		{
			SendMessageEventWriteOut(pSender, (void*)&pRet->data[i], sizeof(SimOne_Data_SensorConfiguration), type);
		}
	}
}
void BridgeIOServer::SendWayPoints(SocketBasePtr pSender, Bridge::EOutSimOneMessageType type)
{
	auto hostVehicleConfig = BridgeConfig::BridgeConfig::getInstance().getHostVehicleConfig();
	for (auto item = hostVehicleConfig.begin(); item != hostVehicleConfig.end(); item++) {
		SimOne_Data_WayPoints *pRet = BridgeConfig::getInstance().wayPointsConfig(item->first);
		SendMessageEventWriteOut(pSender, (void*)pRet, sizeof(SimOne_Data_WayPoints), type);
	}
}
void BridgeIOServer::SendHDMapInfo(SocketBasePtr pSender, Bridge::EOutSimOneMessageType type)
{
	SimOne_Data_Map* pRet = BridgeConfig::getInstance().getHDMapInfo();
	SendMessageEventWriteOut(pSender, (void*)pRet, sizeof(SimOne_Data_Map), type);
}
void BridgeIOServer::SendCaseInfo(SocketBasePtr pSender, Bridge::EOutSimOneMessageType type)
{
	SimOne_Data_CaseInfo* pRet = BridgeConfig::getInstance().getCaseInfo();
	SendMessageEventWriteOut(pSender, (void*)pRet, sizeof(SimOne_Data_CaseInfo), type);
}

void BridgeIOServer::SendCaseStatus(SocketBasePtr pSender, Bridge::EOutSimOneMessageType type)
{
	int pRet = BridgeConfig::getInstance().getCaseStatus();
	SendMessageEventWriteOut(pSender, (void*)&pRet, sizeof(int), type);
}

void BridgeIOServer::SendMainVehicleList(SocketBasePtr pSender)
{
	TaskHotAreaManager::getInstance().SendMainVehicleList(pSender);
}

void BridgeIOServer::sendRouteMessage(SocketBasePtr pSender, void* pBuffer, int length, int commandId)
{
}

void BridgeIOServer::AddConnHostVehicleClient(std::string hostVehicleId, bool isConnected, CybertronSyncConn client) {
	ConnHostVehicleClienMap::iterator it = mConnHostVehicleClienMap.find(hostVehicleId);
	if (it == mConnHostVehicleClienMap.end()){
		mConnHostVehicleClienMap[hostVehicleId] = client;
		logInfo("Bridgeio successfully connected to the host vehicle(%s)", hostVehicleId.c_str());
	}
	else {
		logWarning("A simulation(hostVehicleId:%s) node has been registered", hostVehicleId.c_str());
	}
}
void BridgeIOServer::RemoveConnHostVehicleClient(CybertronSyncConn client) {
	for (auto it = mConnHostVehicleClienMap.begin(); it != mConnHostVehicleClienMap.end();) {
		mConnHostVehicleClienMap.erase(it++);
	}
}
void BridgeIOServer::SendToSimOneInMainVehicleNode(std::string hostVehicleId, void* pBuffer, int length, int commandId) {
	ConnHostVehicleClienMap::iterator it = mConnHostVehicleClienMap.find(hostVehicleId);
	if (it == mConnHostVehicleClienMap.end()){
		return;
	}
	cybertron::proto::sensor::DataVehicleBodyState state;
	it->second.send(proto::sensor::EDataType_VehicleBodyState, state);
}